#include "OOPSMP/Core/StateSpace/Plugin/ArticulatedStateSpace.h"
/*
 *@brief obsolete class, not used anymore
 */
namespace OOPSMP {

	ForwardClassDeclaration(ArticulatedStateSpaceNorm);
	class ArticulatedStateSpaceNorm : ArticulatedStateSpace {

		double computeDistanceStateScalingFactor(void) 
		{
			OOPSMPconst_bbox_t  bbox   = getStateBoundingBox(); 
			const double       *coeffs = getDistanceStateCoeffs();
			double sum_of_squares	   = 0;

			sum_of_squares = OOPSMPsquare(coeffs[0] * (bbox[1] - bbox[0])) +
							 OOPSMPsquare(coeffs[1] * (bbox[3] - bbox[2])) +
							 OOPSMPsquare(coeffs[2] * (bbox[5] - bbox[4]));

			return ( sqrt(sum_of_squares) + coeffs[3] * M_PI );
		}

		double distanceStates(State_const_t x1, State_const_t x2) const
		{
		    ArticulatedPart_t ap  = (ArticulatedPart_t)getCoreRobotData()->getWorkspace()->getMovingPart(getCoreRobotData()->getMovingPartId());   
			const double *coeffs  = getDistanceStateCoeffs();
			long n				  = ap->getStateVectorLength();
			long numLinks		  = ap->getLinkCount();
			double sum_of_squares = 0.0;
			double distance		  = 0.0;
			int partDim			  = ap->getPartDim();
			double * pointsState1 = (double *)calloc( numLinks * partDim, sizeof(double) );
			double * pointsState2 = (double *)calloc( numLinks * partDim, sizeof(double) );

			// get representative points for each link in state x1
			applyState(x1);
			for ( int i = 0 ; i < numLinks ; i++ )
				ap->getSomePointForLink( i, pointsState1 + 3*i );

			// get representative points for each link in state x2
			applyState(x2);
			for ( int i = 0 ; i < numLinks ; i++ )
				ap->getSomePointForLink( i, pointsState2 + 3*i );

			// calculate distances between the points of each link in two states and sum them up
			for ( int i = 0 ; i < numLinks ; i++ ) {
				sum_of_squares = OOPSMPsquare(coeffs[0] * (pointsState2[3*i + 0] - pointsState1[3*i + 0])) +
								 OOPSMPsquare(coeffs[1] * (pointsState2[3*i + 1] - pointsState1[3*i + 1])) +
								 OOPSMPsquare(coeffs[2] * (pointsState2[3*i + 2] - pointsState1[3*i + 2]));
				distance += sqrt( sum_of_squares );
			}

			// average all the distances and scale
			distance = (distance / numLinks ) * getDistanceStateScalingFactor();

			return distance;
		}
	
	};

	DeclareInstanceFactory(ArticulatedStateSpaceNorm, ArticulatedStateSpaceFactory );
}